#include "includes.h"

extern uint16 amp[4];

extern int amp_time;
int trace[AMP_TIME];

int is_intrack(int intrack){
    switch(intrack){
        case 1:
            if(amp[0]>=AMP_IN_TRACK_LOW&&amp[1]>=AMP_IN_TRACK_LOW)
                return 1;
            else 
                return 0; /* locked */
            break;
        case 0:
            if(amp[2]>=AMP_IN_TRACK_LOW_STRICT&&amp[3]>=AMP_IN_TRACK_LOW_STRICT)
                return 1; /* unlocked */
            else
                return 0;
            break;
    }
    return 0;
}

int is_instraight(void){
    if(amp[0]>=AMP_IN_STRAIGHT&&amp[1]>=AMP_IN_STRAIGHT)
        return 1;
    return 0;
}

int is_lost(int islost){
    if(!islost&&(amp[0]<=AMP_LOST&&amp[1]<=AMP_LOST))
        return 1; /* is lost */
    if(islost&&amp[0]>=AMP_BACK&&amp[1]>=AMP_BACK&&amp[2]>=AMP_M_BACK&&amp[3]>=AMP_M_BACK)
        return 0; /* is back */
    return islost; /* keep status as it is */
}

int amp_classfy(void){
    int i,j;
    double sum,p,q;
    static double IW[10][4]={
        {1.394200, -1.284500, 0.718600, 1.112500},
        {2.212700, 1.358600, -1.163100, 0.301000},
        {-1.994000, 2.498700, 1.837600, -0.675300},
        {2.181400, 0.231800, 2.202800, -0.586100},
        {-0.819200, 0.554300, 0.885300, -3.364900},
        {-1.469700, -1.308500, 0.944700, 1.181900},
        {-1.167700, -0.537500, 1.516900, 1.450200},
        {0.167300, 2.281100, -0.596100, -0.937900},
        {1.589100, 1.011400, 0.537500, 1.568900},
        {1.343200, 1.325700, -0.937200, -1.333800}
    };
    static double LW[10]={
        0.645100,
        2.141900,
        2.014900,
        3.199300,
        -2.780400,
        0.054400,
        0.562900,
        -0.013800,
        -0.515000,
        -0.799400
    };
    static double b[2][10]={
        {-2.590900, -2.431900, 0.235100, -2.250700, -0.321200, -0.297600, -0.974600, 1.229300, 1.893700, 2.481100},
        {-1.402000}
    };
    double x[10];
    for(i=0; i<10; ++i){
        x[i]=b[0][i];
        for(j=0; j<10; ++j)
            x[i]+=IW[i][j]*amp[j];
        p=exp(x[i]);
        q=1/p;
        x[i]=(p-q)/(p+q);
    }
    sum=b[1][0];
    for(i=0; i<10; ++i)
        sum+=LW[i]*x[i];
    sum+=b[1][0];
    p=exp(sum);
    q=1/p;
    return (p-q)/(p+q)>=0.5?1:0;
}

void update_trace(void){
    trace[amp_time]=amp_classfy();
}
